// #include <ros/ros.h>
// #include <sensor_msgs/PointCloud2.h>
// #include <sensor_msgs/CompressedImage.h>

// #include <image_transport/image_transport.h>
// #include <cv_bridge/cv_bridge.h>
// #include <sensor_msgs/image_encodings.h>

// #include <message_filters/subscriber.h>
// #include <message_filters/synchronizer.h>
// #include <message_filters/sync_policies/approximate_time.h>

// #include <pcl_conversions/pcl_conversions.h>
// #include <opencv2/opencv.hpp>

// void callback(const sensor_msgs::CompressedImageConstPtr input_image_msg,
//               const sensor_msgs::PointCloud2ConstPtr input_cloud_msg)
// {
//     std::cout<<"datas get"<<std::endl;
//   std_msgs::Header image_header = input_image_msg->header;
//   std_msgs::Header cloud_header = input_cloud_msg->header;

//   // sensor_msgs to cv image
//   cv::Mat input_image;
//   try
//   {
//     input_image = cv::imdecode(cv::Mat(input_image_msg->data), 1);
//   }
//   catch (cv_bridge::Exception& e)
//   {
//     std::cout << "could not convert sensor_msg image into opencv image!" << std::endl;
//     return;
//   }

//   // sensor_msgs to point cloud
//   pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
//   pcl::fromROSMsg(*input_cloud_msg, *input_cloud_ptr);
//   if (input_cloud_ptr->size() == 0)
//   {
//     std::cout << "input cloud is empty, please check it out!" << std::endl;
//     return;
//   }

// }

// int main(int argc, char** argv)
// {
//   ros::init(argc, argv, "vision_node");
//   ros::NodeHandle nh;
//   ros::NodeHandle priv_nh("~");

//   std::string camera_topic = "/camera3_bgr/compressed";
//   std::string lidar_topic = "/pointcloud_lidar_ls1280";
//   message_filters::Subscriber<sensor_msgs::CompressedImage> camera_sub(nh, camera_topic, 10);
//   message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh, lidar_topic, 10);
//   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CompressedImage, sensor_msgs::PointCloud2>
//       MySyncPolicy;
//   message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), camera_sub, lidar_sub);
//   sync.registerCallback(boost::bind(&callback, _1, _2));
//   ros::spin();

//   return 0;
// }

// msg_filter.cpp
#include <pcl/io/pcd_io.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CompressedImage.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

using namespace std;
std::string point_topic = "/vlp1";
std::string point_topic2 = "/vlp2";

// rosbag::Bag bag_record;//文件直接记录

ros::Publisher pointcloud_pub;
ros::Publisher pointcloud_pub2;

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> testSyncPolicy;

template <typename Point>
void pubpc(ros::Publisher &pub, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    pub.publish(msgs);
}

int i = 0;
const int jump = 1;
//回调函数
void callback(const sensor_msgs::PointCloud2ConstPtr &point, const sensor_msgs::PointCloud2ConstPtr &point2) //回调中包含多个消息
{
    if ((i++) % jump != 0)
        return;
    //以实时发布的方式发布
    pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*point, *frames);
    pcl::PointCloud<pcl::PointXYZI>::Ptr frames2(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*point2, *frames2);
    pubpc(pointcloud_pub, *frames);
    pubpc(pointcloud_pub2, *frames2);

    // 生成pcd
    pcl::PCDWriter writer;
    int id = i / jump;
    cout << "writing NO: " << id << endl;
    std::string name = std::to_string(id + 202300000);
    writer.write("/home/zwh/ws/gazebo/bags/vlp/front/" + name + ".pcd", *frames);
    writer.write("/home/zwh/ws/gazebo/bags/vlp/left/" + name + ".pcd", *frames2);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "time_synch");
    ros::NodeHandle n;

    //发布的话题
    pointcloud_pub = n.advertise<sensor_msgs::PointCloud2>(point_topic, 1000);
    pointcloud_pub2 = n.advertise<sensor_msgs::PointCloud2>(point_topic2, 1000);
    //订阅的话题
    //   std::string camera_topic = "/camera3_bgr/compressed";
    //   std::string lidar_topic = "/pointcloud_lidar_ls1280";
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub(n, "/velodyne2_points", 10); // topic1 输入
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub2(n, "/velodyne_points", 10); // topic2 输入

    message_filters::Synchronizer<testSyncPolicy> sync(testSyncPolicy(10), pointcloud_sub, pointcloud_sub2); // 同步
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();
    ros::shutdown();

    return 0;
}